/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: IMUClientExample.cpp,v 1.5 2009-08-27 14:01:52 morsko Exp $
 *
 * \file
 * \brief This is an example for using the IMU interface.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#include "IMUClient.hpp"
#include <curses.h>
#include <math.h>
//*****************************************************************************
#define GIMI_CLIENT_API 20000
#include "gimi.h"
//*****************************************************************************
// Objects
static bool run = true;
static gimi::GIMI g;
static MaCI::IMU::CIMUClient imu(&g, 0);
//*****************************************************************************
//*****************************************************************************
//static const char *strOK = "OK.\n";
//static const char *strFail = "FAILED!\n";
//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
//*****************************************************************************
static void DoIMU()
{

  // Initialize Curses (After this point, don't use printf family calls!)
  initscr(); 
  cbreak(); 
  //noecho();
  nonl();
  intrflush(stdscr, FALSE);
  keypad(stdscr, TRUE);
  scrollok(stdscr, TRUE);
  //nodelay(stdscr, TRUE);
  //halfdelay(10);
  
  // After this point, we don't want dPrint to print to Screen, so we disable it.
  debugSetPrintLevel(0);
  debugSetLogLevel(6);
  
  clear();
  printw("Welcome to IMU test program.\n\n");
  int accSeq = -1;
  int velSeq = -1;
  int angSeq = -1;
  int qSeq = -1;
  int oMSeq = -1;
  do {
    
    int value;
    printw("\nSelect IO Query:\n");
    printw("a = Get accelerations\n"
	   "b = Get angular velocities\n"
	   "c = Get angles\n"
	   "d = Get Quaternion\n"
           "e = Get Orientation Matrix\n"
           "q = QUIT\n");
    printw("?\n");
    value = getch();
    clear(); // After input, clear the screen. Not after processing!
    switch(value){
    case 'a':{
     MaCI::IMU::TAcceleration3d acc;
     MaCI::Common::TTimestamp tstamp;
      if(imu.GetAccelerations(acc,tstamp,&accSeq,1000)){
        printw("Got accelerations(timestamp %u %u): x %f, y %f, z %f",
               tstamp.timestamp_s,
               tstamp.timestamp_us,
               acc.accelerationX,
               acc.accelerationY,
               acc.accelerationZ);
      }else{
        printw("failed to get accelerations!");
      }
      break;
    }
    case 'b':{
      MaCI::IMU::TAngularVelocity3d vel;
      MaCI::Common::TTimestamp tstamp;
      if(imu.GetAngularVelocities(vel,tstamp,&velSeq,1000)){
        printw("Got Angular Velocities (timestamp %u %u: wx %f, wy %f, wz %f",
               tstamp.timestamp_s,
               tstamp.timestamp_us,
               vel.wX,
               vel.wY,
               vel.wZ);

      }else{
        printw("failed to get angular velocities!");
      }
      break;
    }
    case 'c':{
      MaCI::IMU::TAngle3d ang;
      MaCI::Common::TTimestamp tstamp;
      if(imu.GetAngles(ang,tstamp,&angSeq,1000)){
        printw("Got angles (timestamp %u %u): roll %f, pitch %f, yaw %f",
               tstamp.timestamp_s,
               tstamp.timestamp_us,
               ang.roll,
               ang.pitch,
               ang.yaw);

      }else{
        printw("failed to get angles!");
      }
      break;
    }
    case 'd':{
      MaCI::IMU::TQuaternion qua;
      MaCI::Common::TTimestamp tstamp;
      if(imu.GetQuaternion(qua,tstamp,&qSeq,1000)){
        printw("Got quaternion (timestamp %u %u): scalar %f, x %f,y %f,z %f",
               tstamp.timestamp_s,
               tstamp.timestamp_us,
               qua.scalar,
               qua.x,
               qua.y,
               qua.z);

      }else{
        printw("failed to get quaternions!");
      }
      break;
    }
    case 'e':{
      MaCI::IMU::TOrientationMatrix ori;
      MaCI::Common::TTimestamp tstamp;
      if(imu.GetOrientationMatrix(ori,tstamp,&oMSeq,1000)){
        printw("Got Orientation matrix (timestamp %u %u:\n" 
               "|e11 %f\t, e12 %f\t,e13 %f|\n"
               "|e21 %f\t, e22 %f\t,e23 %f|\n"
               "|e31 %f\t, e32 %f\t,e33 %f|\n",
               tstamp.timestamp_s,
               tstamp.timestamp_us,
               ori.e11,
	       ori.e12,
	       ori.e13,
	       ori.e21,
	       ori.e22,
	       ori.e23,
	       ori.e31,
	       ori.e32,
	       ori.e33);
                
             

      }else{
        printw("failed to get quaternions!");
      }
      break;
      break;
    }
    case 'q':
     
      run = false;
      printw("\nBYE BYE!.\n\n");
      
      break;

    default:
      clear();
      printw("\nUnknown command.\n\n");
      continue;
      break;
    }
    
  } while(run);
  
  // Close curses.
  endwin();
}

//*****************************************************************************
void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tIOClientExample [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-s name]\t\t\tUse 'name' as SpeedCtrl target\n");
  fprintf(stderr,"  \t\t\t\tOR\n");
  fprintf(stderr,"  [-S name]\t\t\tSpecify the data source by MaCISL\n\n");
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

int main(int argc, char *argv[]){
 // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 40002;
  std::string gimnetName = "";
  std::string gimnetSourceName = "";
  std::string datasourceMaCISL;
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(8);
  debugSetLogFilename("IMU_Client_example.log");
  debugSetLogLevel(10);
  // Greet
  fprintf(stderr,
	  "\nIOClientExample - MaCI IO Interface Example - v0.0\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "s:S:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 's':
      gimnetSourceName = optarg;
      dPrint(3,"Using GIMnet source name '%s'", optarg);
      break;
      
    case 'S':
      datasourceMaCISL = optarg;
      dPrint(3,"Using DataSource MaCISL '%s'", optarg);
      break;

    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  MaCI::EMaCIError e;
  
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
   
    // Assign default target.
    if (gimnetSourceName.size()) {
      imu.SetDefaultTarget(gimnetSourceName);
      
    } else {
      // GIMI is Open(), so we can execute this.
      MaCI::MaCICtrl::SMaCISL sl(datasourceMaCISL);
      if (sl.IsValidFQMaCISL()) {
        imu.SetDefaultTarget(sl, 10000);
      }
    
    }
    // Attempt to Open Client
    if ( (e = imu.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open IMUClient instance! (%s)",
	     GetErrorStr(e).c_str());
      
    } else {
      DoIMU();
      imu.Close();
    }
  }
  return 0;
}
